#include "DiffMuscle3.h"
#include <OpenSim/Common/SimmMacros.h>
#include <OpenSim/Common/DebugUtilities.h>
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Common/Storage.h>
#include <iostream>

using namespace std;
using namespace OpenSim;
using namespace SimTK;

const string DiffMuscle3::STATE_ACTIVATION_FAST_NAME = "activation_fast";
const string DiffMuscle3::STATE_ACTIVATION_DERIVATIVE_FAST_NAME = "activation_velocity_fast";


void DiffMuscle3::setNull()
{
	Super::setNull();
}

void DiffMuscle3::constructProperties() 
{
	//Super::constructProperties();
	
	MuscleFirstOrderActivationDynamicModel actMdl;
    actMdl.setMinimumActivation(0.01);
	constructProperty_activationModelFast( actMdl );
	
	MuscleSecondOrderActivationDynamicModel act2Mdl;
    actMdl.setMinimumActivation(0.01);
    constructProperty_activationModel2ndOrderFast(act2Mdl);

	constructProperty_default_activation_fast(actMdl.getMinimumActivation());

	constructProperty_maxContractionVelocityFast(10.0);

	constructProperty_scaleFactorFast(0.1);
	constructProperty_scaleFactorSlow(0.9);
}

void DiffMuscle3::buildMuscle()
{
	//Super::buildMuscle();

	//Ensure all of the object names are uptodate
    std::string aName = getName();

    std::string tmp = aName;
    tmp.append("_activationModelFast");
    MuscleFirstOrderActivationDynamicModel& actMdl 
        = upd_activationModelFast();
    actMdl.setName(tmp);

    tmp = aName;
    tmp.append("_activationModelFast2ndOrder");
    MuscleSecondOrderActivationDynamicModel& act2Mdl 
        = upd_activationModel2ndOrderFast();    
    act2Mdl.setName(tmp);

	actMdl.ensureModelUpToDate();
	act2Mdl.ensureModelUpToDate();

	setObjectIsUpToDateWithProperties();
}


void DiffMuscle3::ensureMuscleUpToDate()
{
	if(isObjectUpToDateWithProperties() == false)
    {
        buildMuscle() ; 
    }
}


//=============================================================================
// CONSTRUCTOR(S)
//=============================================================================

DiffMuscle3::DiffMuscle3():
	Millard2012EquilibriumMuscle()
{
	setNull();
    constructProperties();
    ensureMuscleUpToDate();
}

DiffMuscle3::DiffMuscle3( const std::string &aName,
                 double aMaxIsometricForce,
                 double aOptimalFiberLength,
                 double aTendonSlackLength,
                 double aPennationAngle ) :
	Millard2012EquilibriumMuscle( aName,aMaxIsometricForce,aOptimalFiberLength,aTendonSlackLength,
                               aPennationAngle)
	{
		setNull();
		constructProperties();

		setName(aName);    
		setMaxIsometricForce(aMaxIsometricForce);
		setOptimalFiberLength(aOptimalFiberLength);
		setTendonSlackLength(aTendonSlackLength);
		setPennationAngleAtOptimalFiberLength(aPennationAngle);
    
		ensureMuscleUpToDate();
	}


double DiffMuscle3::getDefaultActivationFast() const
{
	double defaultActivationFast = 0;


    if(get_use_second_order_activation()){
        const MuscleSecondOrderActivationDynamicModel& act2Mdl = 
                    get_activationModel2ndOrderFast();
        defaultActivationFast =  
            act2Mdl.clampActivation(get_default_activation_fast());
    }else{
        const MuscleFirstOrderActivationDynamicModel& actMdl = 
                    get_activationModelFast();
        defaultActivationFast =  
            actMdl.clampActivation(get_default_activation_fast());
    }
    return defaultActivationFast;
}


void DiffMuscle3::setDefaultActivationFast( double activationFast )
{
	double defaultAct = 0;

    if(get_use_second_order_activation()){
        const MuscleSecondOrderActivationDynamicModel &act2Mdl = 
            get_activationModel2ndOrderFast();
            set_default_activation_fast(act2Mdl.clampActivation(activationFast));      
    }else{
        const MuscleFirstOrderActivationDynamicModel &actMdl = 
            get_activationModelFast();
        set_default_activation_fast(actMdl.clampActivation(activationFast));  
    }
    ensureMuscleUpToDate();
}

/*
void DiffMuscle3::connectToModel(Model& model)
{
    Super::connectToModel(model);
    ensureMuscleUpToDate();
}
*/

void DiffMuscle3::setActivationFast( SimTK::State& s, double activationFast ) const
{
	SimTK_ASSERT(isObjectUpToDateWithProperties()==true,
        "DiffMuscle3: Muscle is not"
        " to date with properties");

    if(isActivationAState())
    {        
        if(get_use_second_order_activation()){
                const MuscleSecondOrderActivationDynamicModel& act2Mdl 
                    = get_activationModel2ndOrderFast();
                double clampedActivation=act2Mdl.clampActivation(activationFast);
                setStateVariable(s,STATE_ACTIVATION_FAST_NAME,clampedActivation);    
                markCacheVariableInvalid(s,"velInfo");
                markCacheVariableInvalid(s,"dynamicsInfo");
				markCacheVariableInvalid(s, "dynamicsInfoFast");
        }else{
            const MuscleFirstOrderActivationDynamicModel& actMdl 
                = get_activationModelFast();
            double clampedActivation=actMdl.clampActivation(activationFast);
            setStateVariable(s,STATE_ACTIVATION_FAST_NAME,clampedActivation);    
            markCacheVariableInvalid(s,"velInfo");
            markCacheVariableInvalid(s,"dynamicsInfo");
			markCacheVariableInvalid(s, "dynamicsInfoFast");
        }
    }    
}



void DiffMuscle3::addToSystem(SimTK::MultibodySystem& system) const
{
    Super::addToSystem(system);

	double value = 0.0;

    if(isActivationAState()){
        addStateVariable(STATE_ACTIVATION_FAST_NAME);     
                addCacheVariable(STATE_ACTIVATION_FAST_NAME+"_deriv", 
                value, 
                SimTK::Stage::Dynamics);
    
        if(get_use_second_order_activation()){
        addStateVariable(STATE_ACTIVATION_DERIVATIVE_FAST_NAME);     
                addCacheVariable(STATE_ACTIVATION_DERIVATIVE_FAST_NAME+"_deriv", 
                value, 
                SimTK::Stage::Dynamics);
        }
    }

	addCacheVariable<DiffMuscle3::MuscleDynamicsInfoFast>
       ("dynamicsInfoFast", MuscleDynamicsInfoFast(), SimTK::Stage::Dynamics);
}
   

void DiffMuscle3::connectToModel(Model& model)
{
    Super::connectToModel(model);
    ensureMuscleUpToDate(); 
}

 
void DiffMuscle3::initStateFromProperties(SimTK::State& s) const
{
    Super::initStateFromProperties(s);

    if(isActivationAState()){
        setActivationFast(s, getDefaultActivationFast());   

        //First time derivative of activation is set to zero ...
        if(get_use_second_order_activation()){
            setStateVariable(s,STATE_ACTIVATION_DERIVATIVE_FAST_NAME,0.0);    
            markCacheVariableInvalid(s,"velInfo");
            markCacheVariableInvalid(s,"dynamicsInfo");  
        }
    }
}
    
void DiffMuscle3::setPropertiesFromState(const SimTK::State& s)
{
    Super::setPropertiesFromState(s);

    if(isActivationAState()){
        setDefaultActivationFast(getStateVariable(s,STATE_ACTIVATION_FAST_NAME));        
    }

    ensureMuscleUpToDate();
}


void DiffMuscle3::setActivationModelFast( MuscleFirstOrderActivationDynamicModel& aActivationMdlFast )
{
    set_activationModelFast(aActivationMdlFast);
    ensureMuscleUpToDate();
}

void DiffMuscle3::setSecondOrderActivationModelFast( MuscleSecondOrderActivationDynamicModel& aActivationMdlFast )
{
    set_activationModel2ndOrderFast(aActivationMdlFast);
    ensureMuscleUpToDate();
}

const MuscleFirstOrderActivationDynamicModel& DiffMuscle3::
    getActivationModelFast() const
{
	return get_activationModelFast();
}


const MuscleSecondOrderActivationDynamicModel& DiffMuscle3::getSecondOrderActivationModelFast() const
{
    return get_activationModel2ndOrderFast();                 
}

/*
const ForceVelocityInverseCurve& DiffMuscle3::
    getForceVelocityInverseCurveFast() const
{
    ensureMuscleUpToDate();
    return get_forceVelocityInverseCurveFast();
}

void DiffMuscle3::setForceVelocityInverseCurveFast(
        ForceVelocityInverseCurve& aForceVelocityInverseCurveFast)
{   
    set_forceVelocityInverseCurveFast(aForceVelocityInverseCurveFast);
}
*/

double DiffMuscle3::getScaleFactorFast() const
{
	return get_scaleFactorFast();
}

double DiffMuscle3::getScaleFactorSlow() const
{
	return get_scaleFactorSlow();
}

void DiffMuscle3::setScaleFactorFast( double scf )
{
	set_scaleFactorFast( scf );
}

void DiffMuscle3::setScaleFactorSlow( double scs )
{
	set_scaleFactorSlow( scs );
}

double DiffMuscle3::getActivationFast(const SimTK::State& s) const
{
	return getStateVariable(s, STATE_ACTIVATION_FAST_NAME);
}


void DiffMuscle3::setMaxContractionVelocityFast( double maxContractionVelocityFast )
{
	set_maxContractionVelocityFast( maxContractionVelocityFast );
}

double DiffMuscle3::getMaxContractionVelocityFast() const
{
	return get_maxContractionVelocityFast();
}


SimTK::Vector DiffMuscle3::computeStateVariableDerivatives(
        const SimTK::State& s) const
{
	SimTK::Vector derivs = Super::computeStateVariableDerivatives(s) ;
	
	int idx = derivs.size() -1 ;
	
	derivs[idx] = getActivationDerivativeFast(s, 1);
		
	if( get_use_second_order_activation() )
	{
			derivs[idx] = getActivationDerivativeFast(s, 2);
			derivs[idx-1] = getActivationDerivativeFast(s, 1);
	}
	else{
		derivs[idx] = getActivationDerivativeFast(s, 1);
	}
		
	return derivs;
 }


 double DiffMuscle3::
    getActivationDerivativeFast(const SimTK::State& s, int order) const
{
    
    SimTK_ASSERT(isObjectUpToDateWithProperties()==true,
        "DiffMuscle3: Muscle is not to date with properties");
   
	double activationDerivativeFast = 0;

    if(isActivationAState()){
        activationDerivativeFast = calcActivationDerivativeFast(s,order);
    }else{
        activationDerivativeFast = 0;
    }

    return activationDerivativeFast;
}


double DiffMuscle3::
    getMinimumActivationFast() const
{
    double minActivation = 0 ;

    if(get_use_second_order_activation()){
        const MuscleSecondOrderActivationDynamicModel &act2Mdl 
            = get_activationModel2ndOrderFast();   
        minActivation = act2Mdl.getMinimumActivation();
    }else{
        const MuscleFirstOrderActivationDynamicModel &actMdl 
            = get_activationModelFast();   
        minActivation = actMdl.getMinimumActivation();
    }

    return minActivation;
}


double DiffMuscle3::
            calcActiveFiberForceAlongTendonFast(double activation, 
                                            double fiberLength, 
                                            double fiberVelocity) const
{
    string caller = getName();
    caller.append(  "::DiffMuscle3"
                    "::calcActiveFiberForceAlongTendonFast");

    double activeFiberForce = 0;    
    //If the fiber is in a legal range, compute the force its generating
    if(isFiberStateClamped(fiberLength,fiberVelocity) == false){

        //Clamp activation to a legal range
        const MuscleFirstOrderActivationDynamicModel& actMdl 
            = get_activationModelFast();
        double ca = actMdl.clampActivation(activation);

        //Normalize fiber length and velocity
        double lceN    = fiberLength/getOptimalFiberLength();
        double dlceN   = fiberVelocity / 
                        (getOptimalFiberLength() * getMaxContractionVelocityFast());

        //Get the necessary curves
        const ActiveForceLengthCurve& falCurve
            = get_ActiveForceLengthCurve();
        const FiberForceLengthCurve& fpeCurve
            = get_FiberForceLengthCurve();

        //Evaluate the force active length and force velocity multipliers
        double fal  = falCurve.calcValue(lceN);
        double fv   = fvCurve.calcValue(dlceN);
        double fiso = getMaxIsometricForce();
        double fpe  = fpeCurve.calcValue(lceN);
        //Evaluate the pennation angle
        double phi = penMdl.calcPennationAngle(lceN);

        //Compute the active fiber force 
        Vec3 fiberForceV = calcFiberForce(fiso,ca,fal,fv,fpe,dlceN);
        double fa = fiberForceV[1];
        double faAT = calcFiberForceAlongTendon(fa,cos(phi));
		activeFiberForce = faAT;
    }
    //Compute the active fiber force
    /** David : somthing is missing here, activeFiberForce never assigned */

	return activeFiberForce;


}



/*
double DiffMuscle3::getActivationRateFast(const SimTK::State& s) const
{
    ensureMuscleUpToDate();     
    return calcActivationRateFast(s);
}
*/

double DiffMuscle3::
    calcActivationRateFast(const SimTK::State& s) const 
{    
    double excitation = getExcitation(s);
    double activation = getActivationFast(s);

    const MuscleFirstOrderActivationDynamicModel& actMdl 
        = getProperty_activationModelFast().getValue(); 

    double dadt = actMdl.calcDerivative(activation,excitation);
    return dadt;
}

const DiffMuscle3::MuscleDynamicsInfo& DiffMuscle3::getMuscleDynamicsInfo(const SimTK::State& s) const
{
	if(!isCacheVariableValid(s,"dynamicsInfo")){
		MuscleDynamicsInfo& umdi = updMuscleDynamicsInfo(s);
		calcMuscleDynamicsInfo(s, umdi);
		markCacheVariableValid(s,"dynamicsInfo");
		// don't bother fishing it out of the cache since 
		// we just calculated it and still have a handle on it
		return umdi;
	}

	return getCacheVariable<MuscleDynamicsInfo>(s, "dynamicsInfo");
}


DiffMuscle3::MuscleDynamicsInfo& DiffMuscle3::updMuscleDynamicsInfo(const SimTK::State& s) const
{
	return updCacheVariable<MuscleDynamicsInfo>(s, "dynamicsInfo");
}


//cache variables override

 double  DiffMuscle3::computeActuation(const SimTK::State& s) const
{    
	 SimTK_ASSERT(isObjectUpToDateWithProperties()==true,
        "DiffMuscle3: Muscle is not"
        " to date with properties");

    const MuscleDynamicsInfo& mdi = getMuscleDynamicsInfo(s);
    setForce(s,mdi.tendonForce );
    return( mdi.tendonForce );
}


void DiffMuscle3::computeInitialFiberEquilibrium(SimTK::State& s) const
{
    SimTK_ASSERT(isObjectUpToDateWithProperties()==true,
        "DiffMuscle3: Muscle is not"
        " to date with properties");

    //Elastic tendon initialization routine
    if(isTendonElastic() == false){
        return;
    }

    try{        
        //Initialize activation to the users desired setting
        
        double clampedActivation = getDefaultActivation();

        if(get_use_second_order_activation()){
            const MuscleSecondOrderActivationDynamicModel& act2Mdl 
            = get_MuscleSecondOrderActivationDynamicModel();
            clampedActivation = act2Mdl.clampActivation(clampedActivation);
            setActivation(s, clampedActivation);

			const MuscleSecondOrderActivationDynamicModel& act2MdlFast = get_activationModel2ndOrderFast();
			setActivationFast(s, act2MdlFast.clampActivation(getDefaultActivationFast()) );

        }else{
            const MuscleFirstOrderActivationDynamicModel& actMdl 
            = get_MuscleFirstOrderActivationDynamicModel();
            clampedActivation = actMdl.clampActivation(clampedActivation);
            setActivation(s,clampedActivation);


            const MuscleFirstOrderActivationDynamicModel& actMdlFast
            = get_MuscleFirstOrderActivationDynamicModel();
            setActivationFast(s,actMdlFast.clampActivation(getDefaultActivationFast()));

        }
        //Initialize the multibody system to the initial state vector
        setFiberLength(s, getOptimalFiberLength());

        _model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);

        //Compute an initial muscle state that develops the desired force and
        //shares the muscle stretch between the muscle fiber and the tendon 
        //according to their relative stiffness.    
        double activation = clampedActivation;
		double activationFast = getDefaultActivationFast();

        int flag_status       = -1;
        double solnErr        = SimTK::NaN;
        int iterations		= 0;
        double fiberLength    = SimTK::NaN;
        double fiberVelocity  = SimTK::NaN;
        double tendonForce    = SimTK::NaN;
                   
        //tol is the desired tolerance in Newtons
        double tol = 1e-8*getMaxIsometricForce();  
        if(tol < SimTK::SignificantReal*10){
            tol = SimTK::SignificantReal*10;
        }

        int maxIter = 200;                              
        double pathLength = getLength(s);
        double pathLengtheningSpeed = getLengtheningSpeed(s);
                
        SimTK::Vector soln;
        
        soln = estimateMuscleFiberState(
                activation, activationFast,
                pathLength, pathLengtheningSpeed, 
                tol       , maxIter);
                
        flag_status    = (int)soln[0];
        solnErr        = soln[1];
        iterations     = (int)soln[2];
        fiberLength    = soln[3];
        fiberVelocity  = soln[4];
        tendonForce    = soln[5];
              
        switch(flag_status){

            case 0: //converged, all is normal
            {
				cout << "computeInitialEquilibrum converged\n";
				setForce(s,tendonForce);
		        setFiberLength(s,fiberLength);               

            }break;

            case 1: //lower fiber length bound hit
            {
                cout << "computeInitialEquilibrum lower fiber length bound hit\n";
				setForce(s,tendonForce);
                setFiberLength(s,fiberLength);
            
                std::string muscleName = getName();            
                printf( "\n\nDiffMuscle3 Initialization Message:"
                        " %s is at its minimum length of %f\n",
                        muscleName.c_str(), getMinimumFiberLength());
            }break;

            case 2: //Maximum number of iterations exceeded.
            {
				cout << "computeInitialEquilibrum maximum number of iterations exceeded\n";
                setForce(s,0.0);
                setFiberLength(s,penMdl.getOptimalFiberLength());

                std::string muscleName = getName();
                std::string fcnName = "\n\nWARNING: Millard2012EquilibriumMuscle::"
                                 "computeInitialFiberEquilibrium(SimTK::State& s)";
                    char msgBuffer[1000];
                    int n = sprintf(msgBuffer,
                        "WARNING: No suitable initial conditions found for\n"
                        "  %s: \n"
                        "  by %s \n"
                        "Continuing with an initial fiber force and "
                            "length of 0 and %f\n"
                        "    Here is a report from the routine:\n \n"
                        "        Solution Error      : %f > tol (%f) \n"
                        "        Newton Iterations   : %d of max. iterations (%d)\n"
                        "    Check that the initial activation is valid,"
                            " and that the whole \n"
                        "    length doesn't produce a pennation"
                            " angle of 90 degrees, nor a fiber\n"
                        "    length less than 0:\n"
                        "        Activation          : %f \n" 
                        "        Whole muscle length : %f \n\n", 
                        muscleName.c_str(),
                        fcnName.c_str(), 
                        penMdl.getOptimalFiberLength(),
                        abs(solnErr),
                        tol,
                        iterations,
                        maxIter,
                        activation, 
                        fiberLength);

                    cerr << msgBuffer << endl;
        
            }break;

            default:
                std::string muscleName = getName();            
                printf(
                    "\n\nWARNING: Millard2012EquilibriumMuscle Initialization:"
                    " %s invalid error flag, setting tendon force to 0.0 and "
                    "the fiber length to the optimal fiber length",
                        muscleName.c_str());

                setForce(s,0.0);
                setFiberLength(s,penMdl.getOptimalFiberLength());
        }

    }catch (const std::exception& e) { 
        //If the initialization routine fails in some unexpected way, tell
        //the user and continue with some valid initial conditions
        cerr << "\n\nWARNING: Millard2012EquilibriumMuscle initialization"
                " exception caught:" << endl;
        cerr << e.what() << endl;
        
        cerr << "    Continuing with initial tendon force of 0 \n" << endl;
        cerr << "    and a fiber length equal to the optimal \n" << endl;
        cerr << "    fiber length ...\n\n" 
             << endl;

        setForce(s,0);
		setFiberLength(s,getOptimalFiberLength());

    }      
}



SimTK::Vector DiffMuscle3::
    estimateMuscleFiberState(double aActivation, double aActivationFast,
                    double pathLength, double pathLengtheningSpeed, 
                    double aSolTolerance, int aMaxIterations) const
{
    std::string caller = getName();
    caller.append(".estimateMuscleFiberState");
    //results vector format
    //1: flag (0 = converged,
    //         1 = diverged,
    //         2= no solution due to singularity:length 0, 
    //         3= no solution due to pennation angle singularity    
    //2: solution error (N)
    //3: iterations
    //4: fiber length (m)
    //5: fiber Velocity (N)
    //6: tendon force (N)
    SimTK::Vector results = SimTK::Vector(6);

    //I'm using smaller variable names here to make it possible to write out 
    //lengthy equations
    double ma = aActivation;
	double maf = aActivationFast;
    double ml = pathLength;
    double dml= pathLengtheningSpeed;

    //Shorter version of the constants
    double tsl = getTendonSlackLength();
    double ofl = getOptimalFiberLength();

    double ophi= getPennationAngleAtOptimalFiberLength();
    double penHeight = penMdl.getParallelogramHeight();
    double fiso= getMaxIsometricForce();
    double vmax = getMaxContractionVelocity();//getPropertyValue<double>(VmaxName);
	double vmaxf = getMaxContractionVelocityFast();


	cout << "EstimateMuscleFiberState," << " vmax:" << vmax << " vmaxf:" << vmaxf << endl;

    //Get muscle model specific properties
    const TendonForceLengthCurve& fseCurve 
        = get_TendonForceLengthCurve(); 
    const FiberForceLengthCurve& fpeCurve 
        = get_FiberForceLengthCurve(); 
    const ActiveForceLengthCurve& falCurve
        = get_ActiveForceLengthCurve(); 
   
    //Shorter version of normalized muscle multipliers

    double fse = 0; //Normalized tendon (series element) force
    double fal = 0; //Normalized active force length multiplier
    double fv  = 0; //Normalized force-velocity multiplier
    double fpe = 0; //Normalized parallel element force


    //*******************************
    //Position level
    double lce = 0;
    double tl  = getTendonSlackLength()*1.01;

    lce = clampFiberLength(penMdl.calcFiberLength( ml, tl));        

    double phi      = penMdl.calcPennationAngle(lce);
    double cosphi   = cos(phi);
    double sinphi   = sin(phi);       
    double tlN  = tl/tsl;
    double lceN = lce/ofl;
    
    //Velocity level
    double dtl    = 0;
    

    double dlce   = penMdl.calcFiberVelocity(lce, sinphi, cosphi, 
                                              ml, tl, dml, dtl, caller);
    double dlceN  = dlce/(vmax*ofl);
	double dlceNf = dlce/(vmaxf*ofl);
    
    double dphi   = penMdl.calcPennationAngularVelocity(tan(phi),lce,dlce,
                                                                    caller);

    double dlceAT = penMdl.calcFiberVelocityAlongTendon(lce,dlce,sinphi,cosphi,
                                                        dphi);

    //*******************************
    //Internal Kinematics Related Variables for the loop
    double dphi_d_lce = 0;
    double dtl_d_lce = 0;
    //*******************************
    //Internal Force Related Variables for the loop
    double Fm = 0;			// Fiber force
	double Fmf = 0; 
    double FmAT=0;           // Fiber force along tendon
	double FmATf= 0;
    double Ft = 0;          // Tendon force
    double ferr = 1;        // Solution error
    
    double dFm_dlce     = 0;  // Partial derivative of muscle force w.r.t. lce
	double dFm_dlcef    = 0;
    double dFmAT_dlce   = 0;  // Partial derivative of muscle force along 
                               // tendon w.r.t. lce
	double dFmAT_dlcef  = 0;
    
	double dFmAT_dlceAT = 0;  // Partial derivative of muscle force along
                               // tendon w.r.t. lce along the tendon.
	double dFmAT_dlceATf = 0;

    double dFt_d_lce   = 0;  // Partial derivative of tendon force w.r.t. lce
    double dFt_d_tl   = 0;   // Partial derivative of tendon force w.r.t. tl

    double dferr_d_lce = 0;  // Partial derivative of the solution error w.r.t
                             // lce
    double delta_lce   = 0;  // Chance in lce

    double Ke          = 0;  // Linearized local stiffness of the muscle
        

    double tmp1         = 0;
    double tmp2         = 0;
    double tmp3         = 0;
    //*******************************
    //Initialize the loop
    
    int iter = 0;    
    int minFiberLengthCtr = 0;
    SimTK::Vec3 fiberForceV, fiberForceVf;

    while( abs(ferr) > aSolTolerance
        && iter < aMaxIterations
        && minFiberLengthCtr < 10){

            //Update the multipliers and their partial derivativaes
            fal         = falCurve.calcValue(lceN);
            fpe         = fpeCurve.calcValue(lceN);
            fse         = fseCurve.calcValue(tlN);                                          
            fv          = fvCurve.calcValue(dlceN);
           
            //Compute the force error edited to remove fk, fcphi terms
            fiberForceV   = calcFiberForce(fiso,ma,fal,fv,fpe,dlceN);
			fiberForceVf   = calcFiberForce(fiso,maf,fal,fv,fpe,dlceNf);
            Fm   = fiberForceV[0];
			Fmf   = fiberForceVf[0];
            FmAT = calcFiberForceAlongTendon(Fm,cosphi);
			FmATf= calcFiberForceAlongTendon(Fmf,cosphi);
            
			Ft = fse*fiso;
            ferr  = getScaleFactorSlow()*FmAT+getScaleFactorFast()*FmATf-Ft;

            cout << "estimateMuscleFiberState::getScaleFactorSlow(): "<< getScaleFactorSlow() <<
				"::getScaleFactorFast(): " << getScaleFactorFast() << endl;

            //Compute the partial derivative of the force error w.r.t. lce
            //Fiber: edited to remove fk, fcphi terms
            dFm_dlce   = calcFiberStiffness(fiso,ma,fv,lceN,ofl);
			dFm_dlcef   = calcFiberStiffness(fiso,maf,fv,lceN,ofl);
            //edited to remove fk, fcphi terms
            dFmAT_dlce= calc_DFiberForceAT_DFiberLength(Fm,
                                                        dFm_dlce,
                                                        lce,
                                                        sinphi,
                                                        cosphi);
			dFmAT_dlcef= calc_DFiberForceAT_DFiberLength(Fmf,
                                                        dFm_dlcef,
                                                        lce,
                                                        sinphi,
                                                        cosphi);
            //edited to remove fk, fcphi terms
            dFmAT_dlceAT = calc_DFiberForceAT_DFiberLengthAT(dFmAT_dlce,
                                                              sinphi,
                                                              cosphi,
                                                              lce);

			dFmAT_dlceATf = calc_DFiberForceAT_DFiberLengthAT(dFmAT_dlcef,
                                                              sinphi,
                                                              cosphi,
                                                              lce);

            //Tendon:
            dFt_d_tl    = fseCurve.calcDerivative(tlN,1)*fiso/tsl;
            
            dFt_d_lce   = calc_DTendonForce_DFiberLength(dFt_d_tl,lce,
                                                    sinphi,cosphi,caller);         

            //Error Derivative
            dferr_d_lce = dFmAT_dlce*getScaleFactorSlow()+dFmAT_dlcef*getScaleFactorFast() - dFt_d_lce;

            if(abs(ferr) > aSolTolerance){
                
                //Take a full Newton Step if the derivative is non-zero
                if(abs(dferr_d_lce) > SimTK::SignificantReal){
                    delta_lce   = - ferr/dferr_d_lce;
                    lce         = lce + delta_lce;
                }else{ //If we've stagnated, perturb the current solution            
                    double perturbation = 
                    2.0*((double)rand())/((double)RAND_MAX)-1.0;
                    double lengthPerturbation = 
                        0.5*perturbation*getOptimalFiberLength();
                    lce = lce + lengthPerturbation;
                }

                if(isFiberStateClamped(lce,dlceN)){
                    minFiberLengthCtr++;
                    lce = getMinimumFiberLength();
                }
                    
                //Update position level quantities, only if they won't go 
                //singular
                    
                phi = penMdl.calcPennationAngle(lce);
                    

                sinphi = sin(phi);
                cosphi = cos(phi);
                tl  =penMdl.calcTendonLength(cosphi,lce,ml);
                lceN = lce/ofl;
                tlN  = tl/tsl;

                /*Update velocity level quantities: share the muscle velocity 
                between the tendon and the fiber according to their relative 
                stiffness:
                
                Fm-Ft = 0               :Equilibrium equation    [1]
                d/dt Fm - d/dt Ft = 0   :1 Time derivative       [2]
                lp = lm + lt                : path definition    [3]
                d/dt lp = d/dt lm + d/dt lt :path derivative     [4]
                 
                Computing a linearized model of [2]
                Fm = Fm0 + Km*lceAT                              [5]
                Ft = Ft0 Kt*xt                                   [6]

                Taking its time derivative
                dFm_d_xm = Km*dlceAT + dKm_d_t*lceAT (assume dKm_d_t = 0)  [7]
                dFt_d_xt = Kt*dtl + dKt_d_t*dtl (assume dKt_d_t = 0)       [8]
                
                Subtituting 7 and 8 into 2
                Km dlceAT - Kt dtl = 0

                Using Eqn 4, we have 2 equations in 2 unknowns. Can now solve
                for tendon velocity, or the velocity of the fiber along the 
                tendon

                This is a hueristic. The above assumptions are necessary as 
                computing the partial derivatives of Km or Kt w.r.t. requires 
                acceleration level knowledge, which is not available in 
                general.

                Stiffness of the muscle is the stiffness of the tendon and the 
                fiber (along the tendon) in series

                the if statement here is to handle the special case when the
                negative stiffness of the fiber (which happens in this model)
                is equal to the positive stiffness of the tendon.         
                */                       
                if( abs(dFmAT_dlceAT*getScaleFactorSlow()+dFmAT_dlceATf*getScaleFactorFast()
					+ dFt_d_tl) > SimTK::SignificantReal
                    && tlN > 1.0){
                        Ke      = ((dFmAT_dlceAT*getScaleFactorSlow()+dFmAT_dlceATf*getScaleFactorFast())*dFt_d_tl)
                                   /(dFmAT_dlceAT*getScaleFactorSlow()+dFmAT_dlceATf*getScaleFactorFast() + dFt_d_tl); 
                        dtl     = (1/dFt_d_tl)*Ke*dml;
                   
                }else{
                        dtl     = dml;
                }

                dlce = penMdl.calcFiberVelocity(lce,sinphi,cosphi,
                                                        ml,tl,dml,dtl,caller); 
                dlceN    = dlce/(vmax*ofl);
                dphi = penMdl.calcPennationAngularVelocity(tan(phi),lce,
                                                            dlce,caller);
                dlceAT = penMdl.calcFiberVelocityAlongTendon(lce,dlce,
                                                    sinphi,cosphi,dphi);
                tmp1 = (dml - dtl)-dlceAT;
               
            }
        
        iter++;
    }

    //*******************************    
    //Populate the output vector
    //*******************************
    
    //1: flag (0 = converged
    //         1=diverged, 
    //         2= no solution due to singularity:length 0, 
    //         3= no solution due to pennation angle singularity
    //2: solution Error (N)
    //3: iterations
    //4: fiber length (m)
    //5: passive force (N)
    //6: tendon force (N)
        
    //If the solution converged
    if(abs(ferr) < aSolTolerance){    
        results[0] = 0;
        results[1] = ferr;
        results[2] = (double)iter;
        results[3] = lce;
        results[4] = dlce;
		//results[5] = fse*fiso;

		//david modified to reflect the change of force in fiber
        results[5] = fse*fiso+ferr;
    }else{ 

        //If the fiber length hit its lower bound        
        if(iter < aMaxIterations){     

            lce = getMinimumFiberLength();
            phi = penMdl.calcPennationAngle(lce);
            cosphi = cos(phi);
            tl  = penMdl.calcTendonLength(cosphi,lce,ml);
            lceN = lce/ofl;
            tlN  = tl/tsl;                
            fse = fseCurve.calcValue(tlN);            

            results[0] = 1.0;
            results[1] = ferr;
            results[2] = (double)iter;
            results[3] = lce;
            results[4] = 0;
			/*david modify to reflect fiber force*/
            results[5] = fse*fiso+ferr; 
 
                      
        //If the solution diverged
        }else{
            results[0] = 2.0;
            results[1] = ferr;
            results[2] = (double)iter;
            results[3] = SimTK::NaN;
            results[4] = SimTK::NaN;
            results[5] = SimTK::NaN;     
        } 
    }

    return results;
    
}


/** Get the rate change of activation */
double DiffMuscle3::
    calcActivationDerivativeFast(const SimTK::State& s, int order) const 
{    
    SimTK_ASSERT(isObjectUpToDateWithProperties()==true,
        "DiffMuscle3: Muscle is not"
        " to date with properties");

    double val = 0;

    if(isActivationAState()){        
        if(get_use_second_order_activation()){

            const MuscleSecondOrderActivationDynamicModel& act2Mdl 
            = get_activationModel2ndOrderFast();

            double u    = getExcitation(s);
            double a    = act2Mdl.clampActivation(getActivation(s));   
            double dadt = getStateVariable(s,STATE_ACTIVATION_DERIVATIVE_FAST_NAME);

            switch(order){                
                case 0:{
                           val = a;
                       }break;
                case 1:{
                            val = dadt;
                       }break;
                case 2:{
                            val = act2Mdl.calcDerivative(dadt,a,u);
                       }break;
                default:
                    SimTK_ERRCHK_ALWAYS(false, 
                        "DiffMuscle3::calcActivationDerivative",
                        "Invalid derivative requested");
            }

        }else{
            const MuscleFirstOrderActivationDynamicModel& actMdl         
                = get_activationModelFast();

                double u    = getExcitation(s);
                double a    = actMdl.clampActivation(getActivation(s)); 

                switch(order){                
                    case 0:{
                               val = a;
                           }break;
                    case 1:{
                                val = actMdl.calcDerivative(a,u);
                           }break;
                    default:
                        SimTK_ERRCHK_ALWAYS(false, 
                            "DiffMuscle3::calcActivationDerivative",
                            "Invalid derivative requested");
                }

            }
        
        
    }
    return val;
} 


void DiffMuscle3::calcMuscleDynamicsInfo(const SimTK::State& s, 
                                    MuscleDynamicsInfo& mdi) const
{

	SimTK_ASSERT(isObjectUpToDateWithProperties()==true,
        "DiffMuscle3: Muscle is not"
        " to date with properties");

        double simTime = s.getTime(); //for debugging purposes

    //Get the quantities that we've already computed
        const MuscleLengthInfo &mli = getMuscleLengthInfo(s);
        const FiberVelocityInfo &mvi = getFiberVelocityInfo(s);
		FiberVelocityInfo &mviFast = FiberVelocityInfo();
		calcFiberVelocityInfoFast(s, mviFast);

        
    //Get the properties of this muscle
        double mclLength      = getLength(s);
        double tendonSlackLen = getTendonSlackLength();
        double optFiberLen    = getOptimalFiberLength();
        double fiso           = getMaxIsometricForce();
        double penHeight      = penMdl.getParallelogramHeight();
        const TendonForceLengthCurve& fseCurve 
            = get_TendonForceLengthCurve();
    //Prep strings that will be useful to make sensible exception messages
        std::string muscleName = getName();
        std::string fcnName     = ".calcMuscleDynamicsInfo";
        std::string caller      = muscleName;        
        caller.append(fcnName);

    //=========================================================================
    // Compute required quantities
    //=========================================================================

    //1. Get fiber/tendon kinematic information
    const MuscleFirstOrderActivationDynamicModel& actMdl 
        = get_MuscleFirstOrderActivationDynamicModel();

	const MuscleFirstOrderActivationDynamicModel& actMdlFast
		= getActivationModelFast();

    const MuscleSecondOrderActivationDynamicModel& act2Mdl 
        = get_MuscleSecondOrderActivationDynamicModel();

	const MuscleSecondOrderActivationDynamicModel& act2MdlFast
        = get_activationModel2ndOrderFast();

    double a = 0, af =0;   
    if(isActivationAState()){
        a = getStateVariable(s, STATE_ACTIVATION_NAME);
		af = getStateVariable(s, STATE_ACTIVATION_FAST_NAME);
    }else{
        a = getControl(s);
    }


    if(get_use_second_order_activation()){
        a = act2Mdl.clampActivation(a);
		af = act2MdlFast.clampActivation(a);
    }else{
        a = actMdl.clampActivation(a);
		af = actMdlFast.clampActivation(a);
    }


    double lce      = mli.fiberLength;
    double lceN     = lce/optFiberLen;
    
	double dlce     = mvi.fiberVelocity;
	double dlcef    = mviFast.fiberVelocity;
    
	double dlceN    = mvi.normFiberVelocity;
	double dlceNf   = mviFast.normFiberVelocity;
    
	double phi      = mli.pennationAngle;
    double cosPhi   = mli.cosPennationAngle;
    double sinPhi   = mli.sinPennationAngle;

    double tl   = mli.tendonLength; 
    double dtl  = mvi.tendonVelocity;
    double tlN  = mli.normTendonLength;
   
    double fal  = mli.fiberActiveForceLengthMultiplier;
    double fpe  = mli.fiberPassiveForceLengthMultiplier;
    
	double fv   = mvi.fiberForceVelocityMultiplier;
	double fvf   = mviFast.fiberForceVelocityMultiplier;
  
    double fiberStateClamped = mvi.userDefinedVelocityExtras[0];

	cout << "DiffMuscle3::calcMuscleDynamicsInfo(), dlce:" << dlce << " dlcef: " << dlcef 
		<< " dlceN:" << dlceN << " dlceNf:" << dlceNf << " fv:" << fv << " fvf:" << fvf <<endl;

    //Compute the stiffness of the muscle fiber
    SimTK_ERRCHK1_ALWAYS(lce > SimTK::SignificantReal, fcnName.c_str(),
        "%s: The muscle fiber has a length of 0, and is causing a singularity", 
        muscleName.c_str());
    SimTK_ERRCHK1_ALWAYS(cosPhi > SimTK::SignificantReal, fcnName.c_str(),
        "%s: Pennation angle is 90 degrees, and is causing a singularity", 
        muscleName.c_str());

    double fm           = 0; //total fiber force
	double fmf          = 0;
    
	double aFm          = 0; //active fiber force
	double aFmf = 0;
    double pFm          = 0; //passive fiber force
	double pFmf = 0;
    
	double fmAT         = 0;
    double dFm_dlce     = 0;
    double dFm_dlcef    = 0;

	double dFmAT_dlceAT = 0;
	double dFt_dtl      = 0;
    double Ke           = 0;

    

    if(fiberStateClamped < 0.5){
        SimTK::Vec3 fiberForceV, fiberForceVf;

        fiberForceV   = calcFiberForce(fiso,a,fal,fv,fpe,dlceN);
		
        fm = fiberForceV[0];
        aFm   = fiberForceV[1];
        pFm   = fiberForceV[2];

		fiberForceVf  = calcFiberForce(fiso,af,fal,fvf,fpe,dlceNf);
		fmf = fiberForceVf[0];
        aFmf   = fiberForceVf[1];
        pFmf   = fiberForceVf[2];

        //Every method except the rigid tendon chooses a fiber velocity
        //that ensures that the fiber does not generate a compressive force
        //Here we must enforce that the fiber generates only tensile forces
        //by saturating the damping force generated by the parallel element
        int simMethod = getSimulationMethod();
        if(isTendonElastic() == false){
            if(fm < 0 || fmf <0 ){
                fm = 0;
				fmf = 0;
                pFm= -aFm;
				pFmf = -aFmf;
            }
        }

        fmAT  = calcFiberForceAlongTendon(getScaleFactorSlow()*fm+getScaleFactorFast()*fmf,cosPhi);               
        dFm_dlce = calcFiberStiffness(fiso,a,fv,lceN,optFiberLen);
		dFm_dlcef = calcFiberStiffness(fiso,af,fvf,lceN,optFiberLen);

        dFmAT_dlceAT=calc_DFiberForceAT_DFiberLengthAT(  dFm_dlce*getScaleFactorSlow()+dFm_dlcef*getScaleFactorFast(),sinPhi,
                                                                cosPhi,lce);   
        //Compute the stiffness of the tendon
        if(isTendonElastic()){
            dFt_dtl    = fseCurve.calcDerivative(tlN,1)*(fiso/tendonSlackLen);

            //Compute the stiffness of the whole muscle/tendon complex
            if (    abs(dFmAT_dlceAT*dFt_dtl) > 0.0
                &&  abs(dFmAT_dlceAT+dFt_dtl) > SimTK::SignificantReal){
                Ke = (dFmAT_dlceAT*dFt_dtl)/(dFmAT_dlceAT+dFt_dtl);
            }
        }else{
            dFt_dtl = SimTK::Infinity;
            Ke = dFmAT_dlceAT;
        }
    }
    
    double fse = 0;
    if(isTendonElastic()){
        fse  = fseCurve.calcValue(tlN);
    }else{
        fse  = fmAT/fiso;
    }

	cout << "fm: " << fm << "; fmf: " << fmf << endl ;

    mdi.activation                   = a;
	//mdi.activationFast               = af;
    mdi.fiberForce                   = fm*getScaleFactorSlow()+fmf*getScaleFactorFast(); 
    mdi.fiberForceAlongTendon        = fmAT;
    mdi.normFiberForce               = (fm*getScaleFactorSlow()+fmf*getScaleFactorFast())/fiso;
    mdi.activeFiberForce             = aFm*getScaleFactorSlow()+aFmf*getScaleFactorFast();
    mdi.passiveFiberForce            = pFm*getScaleFactorSlow()+pFmf*getScaleFactorFast();                                        
                                     
    mdi.tendonForce                  = fse*fiso;
    mdi.normTendonForce              = fse;
                                     
    mdi.fiberStiffness               = dFm_dlce*getScaleFactorSlow()+dFm_dlcef*getScaleFactorFast();;
    mdi.fiberStiffnessAlongTendon    = dFmAT_dlceAT;
    mdi.tendonStiffness              = dFt_dtl;
    mdi.muscleStiffness              = Ke;
                                     
    //Check that the derivative of system energy less work is zero within
    //a reasonable numerical tolerance. Throw an exception if this is not true
    
    double dphidt       = mvi.pennationAngularVelocity;
    
    double dFibPEdt     = mdi.passiveFiberForce*mvi.fiberVelocity;
    double dTdnPEdt     = fse*fiso*dtl;

    double dFibWdt      = -mdi.activeFiberForce*mvi.fiberVelocity;
    double dmcldt       = getLengtheningSpeed(s);
    double dBoundaryWdt = mdi.tendonForce * dmcldt;

    double dSysEdt = (dFibPEdt + dTdnPEdt) - dFibWdt - dBoundaryWdt;
    double tol = sqrt(SimTK::Eps);
    
    /////////////////////////////
    //Populate the power entries
    /////////////////////////////
    mdi.fiberActivePower    =   dFibWdt; 
    mdi.fiberPassivePower   =   -(dFibPEdt); 
    mdi.tendonPower         =   -dTdnPEdt;       
    mdi.musclePower         =   -dBoundaryWdt;


}

void  DiffMuscle3::calcFiberVelocityInfoFast(const SimTK::State& s, 
                                FiberVelocityInfo& fvi) const
{
    SimTK_ASSERT(isObjectUpToDateWithProperties()==true,
        "DiffMuscle3: Muscle is not"
        " to date with properties");

    double simTime = s.getTime(); //for debugging purposes

    //Get the quantities that we've already computed
    const MuscleLengthInfo &mli = getMuscleLengthInfo(s);
        
    //Get the static properties of this muscle
    double lenMcl             = getLength(s);
    double dlenMcl            = getLengtheningSpeed(s);
    double tdnSlkLen          = getTendonSlackLength();
    double optFibLen          = getOptimalFiberLength();

    //Prep strings that will be useful to make sensible exception messages
    std::string muscleName = getName();
    std::string fcnName     = ".calcFiberVelocityInfoFast";

    std::string caller      = muscleName;        
    caller.append(fcnName);

    //=========================================================================
    // Compute fv by inverting the force-velocity relationship in the 
    // equilibrium equations
    //=========================================================================

    //1. Get MuscleLengthInfo information  
    double lce  = mli.fiberLength;    
    double lceN = mli.normFiberLength;
    double tl   = mli.tendonLength;
    double tlN  = mli.normTendonLength;
    double phi  = mli.pennationAngle;
    double cosphi=mli.cosPennationAngle;
    double sinphi = mli.sinPennationAngle;

    double fal  = mli.fiberActiveForceLengthMultiplier;
    double fpe  = mli.fiberPassiveForceLengthMultiplier;

    double dlce = SimTK::NaN;
    double dlceN= SimTK::NaN;
    double fv   = SimTK::NaN;
    double fiso = getMaxIsometricForce();

    int simMethod = getSimulationMethod();

    const MuscleFirstOrderActivationDynamicModel& actMdlFast
                = get_activationModelFast();

    const MuscleSecondOrderActivationDynamicModel& act2MdlFast 
                = get_activationModel2ndOrderFast();

    //=========================================================================
    //Rigid Tendon Fiber Velocity Computation
    //=========================================================================
    if( isTendonElastic() == false){  

        double dtldt = 0;
        if(tl <= (tdnSlkLen-SimTK::SignificantReal)){
            dtldt = dlenMcl; //tendon is buckling, and thus has 100% of the 
                             //path velocity
        }
            dlce = penMdl.calcFiberVelocity(lce,
                                    sinphi,cosphi,
                                    lenMcl, tl, 
                                    dlenMcl,dtldt, 
                                    caller);
                
        dlceN = dlce/(optFibLen*getMaxContractionVelocityFast());
        fv = fvCurve.calcValue(dlceN);
    //=========================================================================
    //Approximate Elastic Tendon Fiber Velocity Computation
    //=========================================================================
    }else if( isTendonElastic() && get_use_fiber_damping() == false){
              
			double a = 0;
            if(isActivationAState()){
                a = getStateVariable(s, STATE_ACTIVATION_FAST_NAME);
            }else{
                a = getControl(s);
            }

            if(get_use_second_order_activation()){
                a = act2MdlFast.clampActivation(a);
            }else{
                a = actMdlFast.clampActivation(a);
            }

            const TendonForceLengthCurve& fseCurve 
                = get_TendonForceLengthCurve();
            double fse  = fseCurve.calcValue(tlN);

            SimTK_ERRCHK1_ALWAYS(cosphi > SimTK::SignificantReal, 
                fcnName.c_str(),
                "%s: Pennation angle is 90 degrees, "
                "and is causing a singularity", 
                muscleName.c_str());
            SimTK_ERRCHK1_ALWAYS(a > SimTK::SignificantReal, 
                fcnName.c_str(),
                "%s: Activation is 0, and is causing a singularity", 
                muscleName.c_str());
            SimTK_ERRCHK1_ALWAYS(fal > SimTK::SignificantReal, 
                fcnName.c_str(),
                "%s: The active force length curve value is 0,"
                " and is causing a singularity", muscleName.c_str());
                 
            fv = calcFv(a, fal,fpe,fse,cosphi,caller);

            //3. Evaluate the inverse force velocity curve        
            const ForceVelocityInverseCurve& fvInvCurve 
                = get_ForceVelocityInverseCurve(); 

            dlceN = fvInvCurve.calcValue(fv);
            dlce  = dlceN*getMaxContractionVelocityFast()*optFibLen;

			cout << "isTendonElastic() && get_use_fiber_damping()" << endl;
			cout << "DiffMuscle3::calcFiberVelocityInfoFast(), " << " vmaxf:" << getMaxContractionVelocityFast() 
				<< " fv:" << fv << " dlce:" << dlce << " dlceN:" << dlceN<< endl;

    }else if( isTendonElastic() && get_use_fiber_damping() == true){
            
            double a = 0;
            if(isActivationAState()){
                a = getStateVariable(s, STATE_ACTIVATION_FAST_NAME);
            }else{
                a = getControl(s);
            }

            if(get_use_second_order_activation()){
                a = act2MdlFast.clampActivation(a);
            }else{
                a = actMdlFast.clampActivation(a);
            }

            const TendonForceLengthCurve& fseCurve 
                = get_TendonForceLengthCurve();
            double fse  = fseCurve.calcValue(tlN);
                  
            //Newton solve for fiber velocity
            fv = 1.0;
            dlce = -1;
            dlceN = -1;
            double beta = get_fiber_damping();

            SimTK_ERRCHK1_ALWAYS(beta > SimTK::SignificantReal, 
                fcnName.c_str(),
                "%s: Damping must be greater than 0!", muscleName.c_str());

            
            SimTK::Vec3 fiberVelocityV 
                = calcDampedNormFiberVelocity(fiso,a,fal,fpe,fse,beta, cosphi);

            //If the Newton method converged update the fiber velocity
            if(fiberVelocityV[2] > 0.5){
                dlceN = fiberVelocityV[0];
                dlce  = dlceN*getOptimalFiberLength()
                                *getMaxContractionVelocityFast();
                fv = fvCurve.calcValue(dlceN);
            }else{
                cout << getName() <<
                    " Fiber Velocity Newton Method Did Not Converge" <<endl;
            }
           
        
    }else{
        printf("%s: Muscle Illegally Configured. This should not be "
               "possible to do. If you see this message contact the OpenSim"
               "deveopment team.\n",caller.c_str());
        dlce = SimTK::NaN;
        dlceN = SimTK::NaN;
        fv = SimTK::NaN;
    } 
   
    //7. Compute the other related velocity components
    //double dlceAT = penMdl.
    double tanPhi = tan(phi);
    double dphidt    = penMdl.calcPennationAngularVelocity(tanPhi,lce,
                                                            dlce   ,caller);    
    double dlceAT = penMdl.calcFiberVelocityAlongTendon(lce,
                                                    dlce,sinphi,cosphi, dphidt);
    double dmcldt = getLengtheningSpeed(s);
    double dtl = 0;

    if(isTendonElastic()){
        dtl       = penMdl.calcTendonVelocity(cosphi,sinphi,dphidt,
                                                    lce,  dlce,dmcldt);
    }
  
    //Check if the state of the fiber is clamped or not.
    double fiberStateClamped = 0.0;
    if(isFiberStateClamped(lce,dlce)){
        dlce = 0;
        dlceN = 0;
        dlceAT = 0;
        dphidt = 0;
        dtl = dmcldt;
        fv = 1.0; // to be consistent with a fiber velocity of 0
        fiberStateClamped = 1.0;
    }

    //Populate the struct;
    fvi.fiberVelocity               = dlce;
    fvi.normFiberVelocity           = dlceN;
    fvi.fiberVelocityAlongTendon    = dlceAT;

    fvi.pennationAngularVelocity    = dphidt;

    fvi.tendonVelocity              = dtl;
    fvi.normTendonVelocity = dtl/getTendonSlackLength();

    fvi.fiberForceVelocityMultiplier = fv;

    fvi.userDefinedVelocityExtras.resize(1);
    fvi.userDefinedVelocityExtras[0] = fiberStateClamped;
}